// 相比于TrajectoryGenerator,不固定中间路径点的位置
#ifndef FREE_MIDDLE_WAYPOINTS_GEN_H
#define FREE_MIDDLE_WAYPOINTS_GEN_H

#include "trajectory_optimization/trajectory_generator.h"

namespace Planner
{
    class TrajectoryGeneratorFree : public TrajectoryGenerator
    {
    private:
        void computeMC();

        void computeFixVal(MatrixXd &df, int axis);

        // void computeSingleAxis(const MatrixXd &df, MatrixXd &coeffs, MatrixXd &dp);

        void numFixFree(int &num_fix, int &num_free);

    public:
        // 初始化R\dp\df\AiMC等参数,用于TrajectoryOptimizationFree
        void initArgs(int type);
    };

    void TrajectoryGeneratorFree::numFixFree(int &num_fix, int &num_free)
    {
        num_fix = 6;                  // 仅固定起点和终点
        num_free = 3 * segments_ - 3; // 中间路径点的p\v\a均可改变
    }

    // 公式为d = MC [dfix dfree] M用于消除重复性变量，C为选择矩阵
    // dfix = [p0 v0 a0 pn vn an] dfree = [p1 v1 a1 p2 v2 a2 .. pn-1 vn-1 an-1]
    void TrajectoryGeneratorFree::computeMC()
    {
        MC_ = MatrixXd::Zero(2 * continus_ * segments_, continus_ * (segments_ + 1));
        int num_fix = 6;
        int num_free = 3 * segments_ - 3;
        // 固定第0段:p0\p1\v0\v1\a0\a1
        MC_(0, 0) = 1;
        MC_(1, 6) = 1;
        MC_(2, 1) = 1;
        MC_(3, 7) = 1;
        MC_(4, 2) = 1;
        MC_(5, 8) = 1;
        // 固定第1~segments_-2段
        for (size_t i = 1; i < segments_ - 1; i++)
        {
            // 固定p
            MC_(6 * i, 3 + 3 * i) = 1;
            MC_(6 * i + 1, 6 + 3 * i) = 1;
            // 固定v
            MC_(6 * i + 2, 4 + 3 * i) = 1;
            MC_(6 * i + 3, 7 + 3 * i) = 1;
            // 固定a
            MC_(6 * i + 4, 5 + 3 * i) = 1;
            MC_(6 * i + 5, 8 + 3 * i) = 1;
        }
        // 固定第segments_-1段:pn-1\pn\vn-1\vn\an-1\an
        MC_(6 * segments_ - 6, 3 * segments_) = 1;
        MC_(6 * segments_ - 5, 3) = 1;
        MC_(6 * segments_ - 4, 3 * segments_ + 1) = 1;
        MC_(6 * segments_ - 3, 4) = 1;
        MC_(6 * segments_ - 2, 3 * segments_ + 2) = 1;
        MC_(6 * segments_ - 1, 5) = 1;
    }

    void TrajectoryGeneratorFree::computeFixVal(MatrixXd &df, int axis)
    {
        df = MatrixXd(6, 1);
        df(0, 0) = way_points_[0][axis];
        df(1, 0) = startVel_[axis];
        df(2, 0) = startAcc_[axis];
        df(3, 0) = way_points_[segments_][axis];
        df(4, 0) = goalVel_[axis];
        df(5, 0) = goalAcc_[axis];
    }

    void TrajectoryGeneratorFree::initArgs(int type)
    {
        // 只需要初始化MC_和dfx\y\z即可,只有这些量不随迭代过程发生改变
        type_ = type;
        computeMC();
        computeFixVal(dfx_, 0);
        computeFixVal(dfy_, 1);
        computeFixVal(dfz_, 2);
    }
} // namespace Planner

#endif